// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <cmath>
#include <iostream>

#include <franka/exception.h>
#include <franka/robot.h>

#include "examples_common.h"

/**
 * @example generate_joint_position_motion.cpp
 * An example showing how to generate a joint position motion.
 * 展示关节位置运动发生器, 可以直接调用 examples_common.h 中的 MotionGenerator
 *
 * @warning Before executing this example, make sure there is enough space in front of the robot.
 */

int main(int argc, char** argv) {
    if (argc != 2) {
        std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
        return -1;
    }
    try {
        franka::Robot robot(argv[1]);
        setDefaultBehavior(robot);

        // 将机器人复位
        std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
        MotionGenerator motion_generator(0.5, q_goal);
        std::cout << "WARNING: This example will move the robot! "
                << "Please make sure to have the user stop button at hand!" << std::endl
                << "Press Enter to continue..." << std::endl;
        std::cin.ignore();
        robot.control(motion_generator);
        std::cout << "Finished moving to initial joint configuration." << std::endl;

        // Set additional parameters always before the control loop, NEVER in the control loop!
        // Set collision behavior.
        robot.setCollisionBehavior(
            {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
            {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
            {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
            {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});

        std::array<double, 7> initial_position;
        double time = 0.0;
        robot.control(
            [&initial_position, &time](const franka::RobotState& robot_state,
            franka::Duration period) -> franka::JointPositions {
                time += period.toSec();

                if (time == 0.0) {
                    initial_position = robot_state.q_d;  // 使用上一次的 q_d 作为初始关节位置
                }

                double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));  // 0 -> pi/4 -> 0

                franka::JointPositions output = {{initial_position[0], initial_position[1],
                                                initial_position[2], initial_position[3] + delta_angle,
                                                initial_position[4] + delta_angle, initial_position[5],
                                                initial_position[6] + delta_angle}};

                if (time >= 5.0) {  // 两个周期后停止
                    std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
                    return franka::MotionFinished(output);
                }
                return output;
            });
    } catch (const franka::Exception& e) {
        std::cout << e.what() << std::endl;
        return -1;
    }

    return 0;
}
